#include "main.h"
#include "printf.h"
#include "mt6835.h"

#define MT6835_READ_REG    0x30
#define MT6835_WRITE_REG   0x60
#define MT6835_PROG_EEPROM 0xC0
#define MT6835_AUTO_ZERO   0x50
#define MT6835_BURST_ANGLE 0xA0

// DMA传输缓冲区
volatile uint8_t mt6835_tx_buffer[6] = {MT6835_BURST_ANGLE, 0x03, 0xFF, 0xFF, 0xFF, 0xFF};
volatile uint8_t mt6835_rx_buffer[6];

static uint8_t SPI_RW(uint8_t data) {
    // 等待发送缓冲区就绪
    while (!LL_SPI_IsActiveFlag_TXE(SPI1));
    // 写入数据
    LL_SPI_TransmitData8(SPI1, data);
    // 等待接收缓冲区非空
    while (!LL_SPI_IsActiveFlag_RXNE(SPI1));
    // 读取接收到的数据
    return LL_SPI_ReceiveData8(SPI1);
}

static inline void SPI_CS_Low(void){
    LL_GPIO_ResetOutputPin(SPI_CS_GPIO_Port, SPI_CS_Pin);
}

static inline void SPI_CS_High(void){
    // 等待传输完成
    while (LL_SPI_IsActiveFlag_BSY(SPI1));
    // 拉高CS
    LL_GPIO_SetOutputPin(SPI_CS_GPIO_Port, SPI_CS_Pin);
}

/*
static uint8_t MT6835_ReadReg(uint16_t reg_addr){

    SPI_CS_Low();
    SPI_RW(MT6835_READ_REG | (reg_addr >> 8));
    SPI_RW(reg_addr & 0xff);
    uint8_t rev = SPI_RW(0xFF);
    SPI_CS_High();
    return rev;
}

static void MT6835_WriteReg(uint16_t reg_addr, uint8_t data){
    SPI_CS_Low();
    SPI_RW(MT6835_WRITE_REG | (reg_addr >> 8));
    SPI_RW(reg_addr & 0xff);
    SPI_RW(data);
    SPI_CS_High();
}
// 写EEPROM
// 暂时用不到，尚未验证

int MT6835_ProgEEPROM(uint16_t reg_addr){
    SPI_CS_Low();
    SPI_RW(MT6835_PROG_EEPROM);
    SPI_RW(0);
    uint8_t ack = SPI_RW(0xFF);
    SPI_CS_High();
    if(ack == 0x55){
        printf_("program MT6835 EEPROM success\n");
        return 1;
    }else{
        printf_("program MT6835 EEPROM fail\n");
        return 0;
    }
}

void MT6835_test(void){
    const uint8_t magic = 0x4C;
    MT6835_WriteReg(0x001, 0x4C);
    if(MT6835_ReadReg(0x001) == magic){
        printf_("MT6835 write/read test pass\n");
    }
    printf_("reg3 = %02X\n", MT6835_ReadReg(0x003));
    printf_("reg4 = %02X\n", MT6835_ReadReg(0x004));
    printf_("reg5 = %02X\n", MT6835_ReadReg(0x005));
    printf_("reg6 = %02X\n", MT6835_ReadReg(0x006));
}
*/


// 读取并校验角度（阻塞）
// 如果成功，返回0 ~ 0x1FFFFF
// 如果失败，返回-8 ~ -1
int32_t MT6835_ReadAngle(void) {
    SPI_CS_Low();
    SPI_RW(MT6835_BURST_ANGLE);
    SPI_RW(0x03);
    uint8_t reg003 = SPI_RW(0xFF); // angle[20:13]
    uint8_t reg004 = SPI_RW(0xFF); // angle[12:5]
    uint8_t reg005 = SPI_RW(0xFF); // angle[4:0] status[2:0]
    uint8_t reg006 = SPI_RW(0xFF); // crc[7:0]
    SPI_CS_High();

    // 计算3个字节的CRC8，reg003的最高位最先移位进入，reg004其次，reg005最后，计算完成后，对比与reg006是否一致
    // 校验公式X^8 + X^2 + X + 1
    LL_CRC_ResetCRCCalculationUnit(CRC);
    LL_CRC_FeedData8(CRC, reg003);
    LL_CRC_FeedData8(CRC, reg004);
    LL_CRC_FeedData8(CRC, reg005);
    uint8_t calculated_crc = LL_CRC_ReadData8(CRC);

    if (calculated_crc != reg006) {
        printf_("MT6835 CRC error: calc=0x%02X, read=0x%02X (regs: 0x%02X, 0x%02X, 0x%02X)\n",
               calculated_crc, reg006, reg003, reg004, reg005);
        return -8; // CRC校验失败
    }

    uint8_t status = reg005 & 0x07;
    if(status != 0){
        printf_("MT6835 warning with status = %d\n", status);
        return -status;
    }

    return (reg003 << 13) | (reg004 << 5) | (reg005 >> 3);
}

// 开始读取角度（DMA）
CCMRAM void MT6835_ReadAngle_Start(void)
{  
  /* 拉低CS 此时MT6835锁存数据*/
  LL_GPIO_ResetOutputPin(SPI_CS_GPIO_Port, SPI_CS_Pin);
  /* 每次传输前重新配置DMA的数据长度寄存器
   * 传输完成后，该寄存器的值会清零，因此需要重置 */
  LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_3, 6);
  LL_DMA_SetDataLength(DMA1, LL_DMA_CHANNEL_1, 6);

  LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_1); // RX first
  LL_DMA_EnableChannel(DMA1, LL_DMA_CHANNEL_3); // TX second
}

// 调用MT6835_ReadAngle_Start后等待5-6us，可以使用MT6835_Finish_DMA_Read_Angle读取角度
CCMRAM int32_t MT6835_Finish_DMA_Read_Angle(void)
{
    /* 确认SPI DMA传输完成（实际不需要循环，第一次判断就是已完成状态，为了程序稳健保留这个步骤）*/
    int32_t max_retry = 255;
    while(!LL_DMA_IsActiveFlag_TC1(DMA1)){
        if(--max_retry == 0){
            /* DMA超时错误 */
            return -9;
        }
    }
    LL_DMA_ClearFlag_TC1(DMA1);

    /* 禁用两个DMA通道 */
    LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_3);  // 发送DMA
    LL_DMA_DisableChannel(DMA1, LL_DMA_CHANNEL_1);  // 接收DMA

    /* 拉高CS */
    LL_GPIO_SetOutputPin(SPI_CS_GPIO_Port, SPI_CS_Pin);

    /* 处理接收到的数据 */
    uint8_t reg003 = mt6835_rx_buffer[2]; // angle[20:13]
    uint8_t reg004 = mt6835_rx_buffer[3]; // angle[12:5]
    uint8_t reg005 = mt6835_rx_buffer[4]; // angle[4:0] status[2:0]
    uint8_t reg006 = mt6835_rx_buffer[5]; // crc[7:0]

    /* 计算3个字节的CRC8，reg003的最高位最先移位进入，reg004其次，reg005最后，计算完成后，对比与reg006是否一致 */
    /* 校验公式X^8 + X^2 + X + 1 */
    LL_CRC_ResetCRCCalculationUnit(CRC);
    LL_CRC_FeedData8(CRC, reg003);
    LL_CRC_FeedData8(CRC, reg004);
    LL_CRC_FeedData8(CRC, reg005);
    uint8_t calculated_crc = LL_CRC_ReadData8(CRC);
    int32_t mt6835_angle;
    if (calculated_crc == reg006) {
      uint8_t status = reg005 & 0x07;
      if (status == 0) {
        /* 计算角度值 */
        mt6835_angle = (reg003 << 13) | (reg004 << 5) | (reg005 >> 3);
      } else {
        /* 状态错误 */
        mt6835_angle = -status;
      }
    } else {
      /* CRC错误 */
      mt6835_angle = -8;
    }

    return mt6835_angle;
}
